#include <system.h>
#include "kegscale_mk2.h"
#include "eeprom_utils.h"
#include "utils.h"
#include "ports.h"

#include "pic_utils.h"

//NOTE: 'num_pints' is actually num_pints * 128
//The reason for this is that with a maximum of 99 pints dispalyed,
//you can fit 99 * 5 * 128 into a uint16_t.  This makes the averaging calculation easier
uint16_t num_pints[8];
struct keginfo_s keginfo;

void start_next_freq_capture() {
    uint8_t num_kegs = eeprom_read(NUM_KEGS_ADDR);
    cur_keg = cur_keg + 1;
    if (cur_keg == num_kegs)
        cur_keg = 0;
    uint8_t mux_value = 0;
    if(cur_keg & 4) set_bit(mux_value, MUX_3);
    if(cur_keg & 2) set_bit(mux_value, MUX_2);
    if(cur_keg & 1) set_bit(mux_value, MUX_1);
    //Disable interrupts so that 'MUX_PORT' doesn't change during our update
    start_crit_sec();
    uint8_t mux_port = MUX_PORT;
    MUX_PORT = (mux_port & ~MUX_MASK) | mux_value;
    end_crit_sec();
    start_ccp_trigger = 1;
    ccp1con = 0x07;
}

uint32_t get_ccp_frequency() {
    uint32_t current_ccp;
    uint32_t prev_ccp;
    ASSIGN_24(current_ccp, ccpr1l, ccpr1h, current_ccp_24);
    ASSIGN_24(prev_ccp, last_ccp_8, last_ccp_16, last_ccp_24);
    if (current_ccp_24 < last_ccp_24)
        set_bit(current_ccp, 24);
    return current_ccp - prev_ccp;
}

uint32_t get_keg_level(uint8_t keg_num) {
    while (ccp1con)
        clear_wdt();
    // start capture of selected keg
    cur_keg = keg_num - 1;
    start_next_freq_capture();
    // wait until capture has finished
    while (ccp1con)
        clear_wdt();
    return get_ccp_frequency();
}

void read_keginfo(uint8_t keg_num) {
    uint8_t offset = CALC_KEGINFO_OFFSET(keg_num);
    keginfo.keg_full    = eeprom_uint24_read(offset + KEGINFO_KEG_FULL_OFFSET);
    keginfo.hz_per_pint = eeprom_uint24_read(offset + KEGINFO_HZ_PER_PINT_OFFSET);
    keginfo.max_pints   = eeprom_read(offset + KEGINFO_MAX_PINTS_OFFSET);
    keginfo.keg_empty   = CALC_KEG_EMPTY();
}

void update_values() {
    read_keginfo(cur_keg);
    uint32_t delta = get_ccp_frequency();
#ifdef SIMULATOR
    // provide a breakpoint in case the value is unexpected
    if (delta != 6992)
	asm nop
#endif
    if (delta <= keginfo.keg_empty) {
        num_pints[cur_keg] = 0;
    } else if (delta >= keginfo.keg_full) {
        num_pints[cur_keg] = keginfo.max_pints * 128;
    } else {
        delta = keginfo.keg_full - delta;
        uint16_t cur_pints = (uint16_t)keginfo.max_pints * 128 - (128 * delta / keginfo.hz_per_pint);
        //Average last 5 readings
        uint16_t old_pints = num_pints[cur_keg];
        uint16_t new_pints = (uint16_t)old_pints * 4 + cur_pints + 2;  // add 2 for rounding
        num_pints[cur_keg] = new_pints / 5;
        //if (old_pints < cur_pints / 10 && new_pints % 5)
        //    num_pints[cur_keg]++;
    }
}

